
// ================================================================================================
// -*- C++ -*-
// File: Graph.hpp
// Author: Guilherme R. Lampert
// Created on: 05/10/13
// Brief: Graph type and edge/node types.
// ================================================================================================

#ifndef __GRAPH_HPP__
#define __GRAPH_HPP__

#include "MapCell.hpp"
#include <limits>
#include <cmath>
#include <list>

//-----------------------------------------------------------------------------

// Graph node for use in creating a navigation graph. This node contains
// the position of the node and a pointer to map cell.
class GraphNode {

public:

	// Every node has an index. A valid index is >= 0
	int index;

	// The node's world position:
	Location location;

	// Map cell this node points to.
	MapCell * cell;

	// Constructors / destructor:
	GraphNode() : index(-1), cell(NULL) { }
	GraphNode(int idx, const Location & loc, MapCell * mapCell) : index(idx), location(loc), cell(mapCell) { }
	virtual ~GraphNode() { }
};

//-----------------------------------------------------------------------------

// Class to define an edge connecting two nodes.
class GraphEdge {

protected:

	// An edge connects two nodes. Valid node indexes are always positive.
	int from, to;

public:

	// Constructors / destructor:
	GraphEdge() : from(-1), to(-1) { }
	GraphEdge(int src, int target) : from(src), to(target) { }
	virtual ~GraphEdge() { }

	inline int From() const { return from; }
	inline void SetFrom(int NewIndex) { from = NewIndex; }

	inline int To() const { return to; }
	inline void SetTo(int NewIndex) { to = NewIndex; }

	// The cost of traversing an edge. Huge (99999) value for not traversable; 0 no cost.
	static double Cost(const GraphNode & a, const GraphNode & b)
	{
		if (CellIsWater(*a.cell) || CellIsWater(*b.cell))
		{
			// Can't move to node if it is water.
			return (99999.0);
		}

		const double manhattanDistance = std::abs(a.location.x - b.location.x) + std::abs(a.location.y - b.location.y);
		return (manhattanDistance); // Traversable
	}

	bool operator == (const GraphEdge & rhs) const
	{
		return ((rhs.from == this->from) && (rhs.to == this->to));
	}
	bool operator != (const GraphEdge & rhs) const
	{
		return !(*this == rhs);
	}
};

//-----------------------------------------------------------------------------

// Graph class using the adjacency list representation.
template<class NodeT, class EdgeT>
class SparseGraph {

public:

	// Nested types:
	typedef EdgeT EdgeType;
	typedef NodeT NodeType;
	typedef std::vector<NodeT>    NodeVector;
	typedef std::list<EdgeT>      EdgeList;
	typedef std::vector<EdgeList> EdgeListVector;

private:

	// the nodes that comprise this graph
	NodeVector nodes;

	// a vector of adjacency edge lists. (each node index keys into the
	// list of edges associated with that node)
	EdgeListVector edges;

	// the index of the next node to be added
	int nextNodeIndex;

	// is this a directed graph?
	const bool isDigraph;

	// returns true if an edge is not already present in the graph. Used
	// when adding edges to make sure no duplicates are created.
	bool UniqueEdge(int from, int to) const;

	// iterates through all the edges in the graph and removes any that point
	// to an invalidated node
	void CullInvalidEdges();

public:

	// Constructor:
	SparseGraph(bool digraph = false) :
		nodes(), edges(), nextNodeIndex(0), isDigraph(digraph) { }

	// returns the node at the given index
	const NodeType & GetNode(int idx) const;

	// non const version
	NodeType & GetNode(int idx);

	// const method for obtaining a reference to an edge
	const EdgeType & GetEdge(int from, int to) const;

	// non const version
	EdgeType& GetEdge(int from, int to);

	// retrieves the next free node index
	int GetNextFreeNodeIndex() const { return (nextNodeIndex); }

	// adds a node to the graph and returns its index
	int AddNode(NodeT node);

	// removes a node by setting its index to -1
	void RemoveNode(int node);

	// Use this to add an edge to the graph. The method will ensure that the
	// edge passed as a parameter is valid before adding it to the graph. If the
	// graph is a digraph then a similar edge connecting the nodes in the opposite
	// direction will be automatically added.
	void AddEdge(EdgeT edge);

	// removes the edge connecting from and to from the graph (if present). If
	// a digraph then the edge connecting the nodes in the opposite direction
	// will also be removed.
	void RemoveEdge(int from, int to);

	// returns the number of active + inactive nodes present in the graph
	int NumNodes() const { return nodes.size(); }

	// returns the number of active nodes present in the graph (this method's
	// performance can be improved greatly by caching the value)
	int NumActiveNodes() const
	{
		int count = 0;
		for (unsigned int n = 0; n < nodes.size(); ++n)
		{
			if (nodes[n].index != -1)
				++count;
		}
		return (count);
	}

	// returns the total number of edges present in the graph
	int NumEdges() const
	{
		int tot = 0;
		for (typename EdgeListVector::const_iterator curEdge = edges.begin(); curEdge != edges.end(); ++curEdge)
		{
			tot += curEdge->size();
		}
		return (tot);
	}

	// returns true if the graph is directed
	bool IsDigraph() const { return isDigraph; }

	// returns true if the graph contains no nodes
	bool IsEmpty() const { return nodes.empty(); }

	// returns true if a node with the given index is present in the graph
	bool IsNodePresent(int nd) const;

	// returns true if an edge connecting the nodes 'to' and 'from'
	// is present in the graph
	bool IsEdgePresent(int from, int to) const;

	// Removes all edges and nodes.
	void Clear()
	{
		nextNodeIndex = 0;
		nodes.clear();
		edges.clear();
	}

	// Removes all edges. Keeps the nodes.
	void ClearEdges()
	{
		edges.clear();
	}

	// Inner class EdgeIterator:
	// Class used to iterate through all the edges connected to a specific node.
	class EdgeIterator {
	private:

		typename EdgeList::iterator curEdge;
		SparseGraph<NodeT, EdgeT> & G;
		const int NodeIndex;

	public:

		EdgeIterator(SparseGraph<NodeT, EdgeT> & graph, int node)
			: G(graph), NodeIndex(node)
		{
			curEdge = G.edges[NodeIndex].begin();
		}

		EdgeType * begin()
		{
			curEdge = G.edges[NodeIndex].begin();
			return &(*curEdge);
		}

		EdgeType * next()
		{
			++curEdge;
			if (end()) return NULL;
			return &(*curEdge);
		}

		bool end() const
		{
			return (curEdge == G.edges[NodeIndex].end());
		}
	};
	friend class EdgeIterator;

	// Inner class NodeIterator:
	// Class used to iterate through the nodes in the graph
	class NodeIterator
	{
	private:

		typename NodeVector::iterator curNode;
		SparseGraph<NodeT, EdgeT> & G;

		// if a graph node is removed, it is not removed from the
		// vector of nodes (because that would mean changing all the indices of
		// all the nodes that have a higher index). This method takes a node
		// iterator as a parameter and assigns the next valid element to it.
		void GetNextValidNode(typename NodeVector::iterator& it)
		{
			if (curNode == G.nodes.end() || it->index != -1)
				return;

			while (it->index == -1)
			{
				++it;
				if (curNode == G.nodes.end())
					break;
			}
		}

	public:

		NodeIterator(SparseGraph<NodeT, EdgeT> & graph) : G(graph)
		{
			curNode = G.nodes.begin();
		}

		NodeT * begin()
		{
			curNode = G.nodes.begin();
			GetNextValidNode(curNode);
			return &(*curNode);
		}

		NodeT * next()
		{
			++curNode;
			if (end()) return NULL;
			GetNextValidNode(curNode);
			return &(*curNode);
		}

		bool end() const
		{
			return (curNode == G.nodes.end());
		}
	};
	friend class NodeIterator;
};

//-----------------------------------------------------------------------------

template <class NodeT, class EdgeT>
bool SparseGraph<NodeT, EdgeT>::IsNodePresent(int nd) const
{
    if ((nd >= (int)nodes.size()) || (nodes[nd].index == -1))
    {
		return false;
    }
    else
	{
		return true;
	}
}

template <class NodeT, class EdgeT>
bool SparseGraph<NodeT, EdgeT>::IsEdgePresent(int from, int to) const
{
    if (IsNodePresent(from) && IsNodePresent(from))
    {
       for (typename EdgeList::const_iterator curEdge = edges[from].begin(); curEdge != edges[from].end(); ++curEdge)
       {
          if (curEdge->To() == to) return true;
       }

		return false;
    }
    else
	{
		return false;
	}
}

template <class NodeT, class EdgeT>
const NodeT & SparseGraph<NodeT, EdgeT>::GetNode(int idx) const
{
    assert((idx < (int)nodes.size()) && (idx >=0));
    return nodes[idx];
}

template <class NodeT, class EdgeT>
NodeT & SparseGraph<NodeT, EdgeT>::GetNode(int idx)
{
    assert((idx < (int)nodes.size()) && (idx >=0));
    return nodes[idx];
}

template <class NodeT, class EdgeT>
const EdgeT & SparseGraph<NodeT, EdgeT>::GetEdge(int from, int to) const
{
  assert((from < nodes.size()) &&
         (from >=0)              &&
          nodes[from].index != -1 &&
         "<SparseGraph::GetEdge>: invalid 'from' index");

  assert((to < nodes.size()) &&
         (to >=0)              &&
         nodes[to].index != -1 &&
         "<SparseGraph::GetEdge>: invalid 'to' index");

  for (typename EdgeList::const_iterator curEdge = edges[from].begin();
       curEdge != edges[from].end();
       ++curEdge)
  {
    if (curEdge->To() == to) return *curEdge;
  }

  assert(0 && "<SparseGraph::GetEdge>: edge does not exist");
}

//non const version
template <class NodeT, class EdgeT>
EdgeT & SparseGraph<NodeT, EdgeT>::GetEdge(int from, int to)
{
  assert((from < nodes.size()) &&
         (from >=0)              &&
          nodes[from].index != -1 &&
         "<SparseGraph::GetEdge>: invalid 'from' index");

  assert((to < nodes.size()) &&
         (to >=0)              &&
         nodes[to].index != -1 &&
         "<SparseGraph::GetEdge>: invalid 'to' index");

  for (typename EdgeList::iterator curEdge = edges[from].begin();
       curEdge != edges[from].end();
       ++curEdge)
  {
    if (curEdge->To() == to) return (*curEdge);
  }

  assert(0 && "<SparseGraph::GetEdge>: edge does not exist");
}

template <class NodeT, class EdgeT>
void SparseGraph<NodeT, EdgeT>::AddEdge(EdgeT edge)
{
  //first make sure the from and to nodes exist within the graph
  assert((edge.From() < nextNodeIndex) && (edge.To() < nextNodeIndex) &&
         "<SparseGraph::AddEdge>: invalid node index");

  //make sure both nodes are active before adding the edge
  if ((nodes[edge.To()].index != -1) &&
      (nodes[edge.From()].index != -1))
  {
    //add the edge, first making sure it is unique
    if (UniqueEdge(edge.From(), edge.To()))
    {
      edges[edge.From()].push_back(edge);
    }

    //if the graph is undirected we must add another connection in the opposite
    //direction
    if (!isDigraph)
    {
      //check to make sure the edge is unique before adding
      if (UniqueEdge(edge.To(), edge.From()))
      {
        EdgeT NewEdge = edge;

        NewEdge.SetTo(edge.From());
        NewEdge.SetFrom(edge.To());

        edges[edge.To()].push_back(NewEdge);
      }
    }
  }
}

template <class NodeT, class EdgeT>
void SparseGraph<NodeT, EdgeT>::RemoveEdge(int from, int to)
{
  assert((from < (int)nodes.size()) && (to < (int)nodes.size()) &&
         "<SparseGraph::RemoveEdge>:invalid node index");

  typename EdgeList::iterator curEdge;

  if (!isDigraph)
  {
    for (curEdge = edges[to].begin();
         curEdge != edges[to].end();
         ++curEdge)
    {
      if (curEdge->To() == from){curEdge = edges[to].erase(curEdge);break;}
    }
  }

  for (curEdge = edges[from].begin();
       curEdge != edges[from].end();
       ++curEdge)
  {
    if (curEdge->To() == to){curEdge = edges[from].erase(curEdge);break;}
  }
}

template <class NodeT, class EdgeT>
int SparseGraph<NodeT, EdgeT>::AddNode(NodeT node)
{
  if (node.index < (int)nodes.size())
  {
    //make sure the client is not trying to add a node with the same ID as
    //a currently active node
    assert(nodes[node.index].index == -1 && "<SparseGraph::AddNode>: Attempting to add a node with a duplicate ID");
    nodes[node.index] = node;
    return nextNodeIndex;
  }
  else
  {
    //make sure the new node has been indexed correctly
    assert(node.index == nextNodeIndex && "<SparseGraph::AddNode>:invalid index");
    nodes.push_back(node);
    edges.push_back(EdgeList());
    return nextNodeIndex++;
  }
}

template <class NodeT, class EdgeT>
void SparseGraph<NodeT, EdgeT>::CullInvalidEdges()
{
  for (typename EdgeListVector::iterator curEdgeList = edges.begin(); curEdgeList != edges.end(); ++curEdgeList)
  {
    for (typename EdgeList::iterator curEdge = (*curEdgeList).begin(); curEdge != (*curEdgeList).end(); ++curEdge)
    {
      if (nodes[curEdge->To()].index == -1 ||
          nodes[curEdge->From()].index == -1)
      {
        curEdge = (*curEdgeList).erase(curEdge);
      }
    }
  }
}

template <class NodeT, class EdgeT>
void SparseGraph<NodeT, EdgeT>::RemoveNode(int node)
{
  assert(node < (int)nodes.size() && "<SparseGraph::RemoveNode>: invalid node index");

  //set this node's index to -1
  nodes[node].index = -1;

  //if the graph is not directed remove all edges leading to this node and then
  //clear the edges leading from the node
  if (!isDigraph)
  {
    //visit each neighbour and erase any edges leading to this node
    for (typename EdgeList::iterator curEdge = edges[node].begin();
         curEdge != edges[node].end();
         ++curEdge)
    {
      for (typename EdgeList::iterator curE = edges[curEdge->To()].begin();
           curE != edges[curEdge->To()].end();
           ++curE)
      {
         if (curE->To() == node)
         {
           curE = edges[curEdge->To()].erase(curE);
           break;
         }
      }
    }

    //finally, clear this node's edges
    edges[node].clear();
  }
  else
  {
    //if a digraph remove the edges the slow way
    CullInvalidEdges();
  }
}

template <class NodeT, class EdgeT>
bool SparseGraph<NodeT, EdgeT>::UniqueEdge(int from, int to)const
{
  for (typename EdgeList::const_iterator curEdge = edges[from].begin();
       curEdge != edges[from].end();
       ++curEdge)
  {
    if (curEdge->To() == to)
    {
      return false;
    }
  }

  return true;
}

#endif // __GRAPH_HPP__
